import rospy
from sensor_msgs.msg import JointState

class PuppetArmSim:
    def __init__(self):
        rospy.init_node('puppet_arm_sim', anonymous=True)
        self.pub = rospy.Publisher('/puppet/joint_left', JointState, queue_size=10)

    def publish_joint_states(self):
        # 创建并发布 JointState 消息
        msg = JointState()
        msg.header.stamp = rospy.Time.now()
        msg.name = ["puppet_joint1", "puppet_joint2", "puppet_joint3", "puppet_joint4", "puppet_joint5", "puppet_joint6"]
        msg.position = [1, 2, 3, 4, 5, 6]  # 这里是大臂的位置数据
        self.pub.publish(msg)
        # rospy.loginfo("Published puppet arm position: %s", msg.position)

    def run(self):
        rate = rospy.Rate(30)  # 30 Hz
        while not rospy.is_shutdown():
            self.publish_joint_states()
            rate.sleep()

if __name__ == '__main__':
    controller = PuppetArmSim()
    try:
        controller.run()
    except rospy.ROSInterruptException:
        pass